#include "dap_control.h"
#include "DAP.h"
#include <rtthread.h>
#include "usb_service.h"

static rt_uint8_t dap2usb_buff[DAP_PACK_SIZE] = {0};
static rt_uint16_t dap2usb_size = 0;

static int dap2usb_mb_pool[DAP_PACK_COUNT] = {0};
static struct rt_mailbox dap2usb_mb = {0};

static char dap2usb_stack[DAP_TASK_STACK_SIZE] = {0};
static struct rt_thread dap2usb_task;

static void usb2dap_handler(rt_uint8_t *data, rt_uint16_t len)
{
    rt_mb_send(&dap2usb_mb, (rt_ubase_t)data);
}

static void dap2usb_handler(void *param)
{
    char *rx_data = NULL;
    for(;;)
    {
        if(rt_mb_recv(&dap2usb_mb, (rt_ubase_t *)&rx_data, RT_WAITING_FOREVER) == RT_EOK)
        {
            if(rx_data[0] == ID_DAP_QueueCommands)
            {
                rx_data[0] = ID_DAP_ExecuteCommands;
            }
            dap2usb_size = DAP_ExecuteCommand((const uint8_t *)rx_data, dap2usb_buff);
            usb_service_dap2usb(dap2usb_buff, dap2usb_size);
        }
    }
}

int dap_service_init(void)
{
    DAP_Setup();

    usb_service_usb2dap_reg(usb2dap_handler);

    rt_mb_init(&dap2usb_mb,
               "dap_mb",
               &dap2usb_mb_pool[0],
               DAP_PACK_COUNT,
               RT_IPC_FLAG_FIFO);

    rt_thread_init(&dap2usb_task,
                   "dap", 
                   dap2usb_handler,
                   RT_NULL,
                   &dap2usb_stack[0],
                   DAP_TASK_STACK_SIZE,
                   DAP_TASK_PRIORITY,
                   DAP_TASK_TIMESLICE);
    rt_thread_startup(&dap2usb_task);


    return RT_EOK;
}
INIT_APP_EXPORT(dap_service_init);
